/***************************************************************************************
 * drone_pos_estimator.cpp
 * Author: Wxt
 * Update Time: 2021.06.28
 * Introduction: 
 *                                  1. 获取来自rotors_simulator仿真上传的无人机状态信息
 *                                  1.1 消息来源一：gazebo_sub  来自于  /ground_truth/odometry  里程计(odometry，位置+姿态)
 *                                  1.2 消息来源二：state_from_rotors（声明了析构函数），
 *                                                                    其中订阅了位置position_sub，/odometry_sensor1/position
 *                                                                     (主要查看）速度 vel_sub  /odometry_sensor1/odometry 
 *                                                                     姿态imu_sub /imu 
 *                                                                    与ground_truth不同的是，该值混入了一些噪声（个人认为价值不大）
 *                                  2.  发布：a.    vision_pub                              （暂时无用)无人机位置和偏航角 坐标系 ENU系
 *                                                     b.    drone_state_pub                     无人机状态发布
 *                                                     c.    odom_pub   用于RVIZ         （暂时没有接RVIZ）
 *                                                     d.   message_pub                            发送消息至ground_station_msg
 *                                                     e.   attitude_target_pub               发布消息至ground_station和其他程序（后续不再从rotors获取姿态信息）
 *                                                      f.   position_target_pub               发布消息至ground_station和其他程序（后续不再从rotors获取位置信息）
 *                                                     g.   test_pub                                       用于测试
 * 版本修正：           时间                                修改内容
 *                              2021.06.28                     建立初始版本
 * *****************************************************************************************/
#include <ros/ros.h>

#include <iostream>
#include <Eigen/Eigen>
#include "math_utils.h"
#include"state_from_rotors.h"
#include "drone_control_utils.h"
#include "message_utils.h"
using namespace std;
#define TRA_WINDOW 1000
#define TIMEOUT_MAX 0.05
#define NODE_NAME "drone_pos_estimator"
//---------------------------------------相关参数-----------------------------------------------
int input_source;
float rate_hz;
float yaw_offset;
Eigen::Vector3f pos_offset;
string mav_name;
string node_name = NODE_NAME;
//---------------------------------------gazebo真值相关------------------------------------------
Eigen::Vector3d pos_drone_gazebo;
Eigen::Quaterniond q_gazebo;
Eigen::Vector3d Euler_gazebo;

//---------------------------------------发布相关变量--------------------------------------------
ros::Publisher vision_pub;
ros::Publisher drone_state_pub;
ros::Publisher message_pub;
uav_drone_msgs::Message message;
ros::Publisher odom_pub;
ros::Publisher trajectory_pub;
uav_drone_msgs::DroneState Drone_State;
nav_msgs::Odometry Drone_odom;
ros::Publisher test_pub;
std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
/************************替换（移植）发布相关变量***************************
 * 本意是在接收来自rotor_simulator 仿真中上报数据的同时，将这些数据信息进行整理
 * 整理后按照类似于Mavros消息中的AttitudeTarget和PositionTarget发布如下信息
 * AttitudeTarget ：四元数（表示姿态），（角速率）
 * PositionTarget：XYZ位置，XYZ速度
 * *************************************************************************/
ros::Publisher attitude_target_pub;
ros::Publisher position_target_pub;
//--------------------------------------函数声明--------------------------------------------
void send_to_fcu();
void pub_to_nodes(uav_drone_msgs::DroneState State_from_fcu);

void gazebo_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    //目前没有进入到else中，else中提示的是警告
    if (msg->header.frame_id == "world")
    {
        pos_drone_gazebo = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);

        q_gazebo = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Euler_gazebo = quaternion_to_euler(q_gazebo);
    }
    else
    {
        pub_message(message_pub, uav_drone_msgs::Message::WARN, NODE_NAME, "wrong gazebo ground truth frame id.");
    }
}

void timerCallback(const ros::TimerEvent &e)
{
    //timerCallback 10s执行一次
    pub_message(message_pub, uav_drone_msgs::Message::NORMAL, NODE_NAME, "Program is running.");
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "drone_pos_estimator");
    ros::NodeHandle nh("~");

    //读取参数表中的参数
    // 定位数据输入源 
    nh.param<int>("input_source", input_source, 0);
    //　程序执行频率
    nh.param<float>("rate_hz", rate_hz, 10);
    //　定位设备偏移量
    nh.param<float>("offset_x", pos_offset[0], 0);
    nh.param<float>("offset_y", pos_offset[1], 0);
    nh.param<float>("offset_z", pos_offset[2], 0);
    nh.param<float>("offset_yaw", yaw_offset, 0);
    nh.param<float>("offset_yaw", yaw_offset, 0);
    nh.param<string>("mav_name", mav_name, "firefly");
    // 【订阅】gazebo仿真真值
    ros::Subscriber gazebo_sub = nh.subscribe<nav_msgs::Odometry>("/"+mav_name+"/ground_truth/odometry", 100, gazebo_cb);

    // 【发布】无人机位置和偏航角 坐标系 ENU系
    vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/"+mav_name+"/simulator_pose/pose", 10);

    // 【发布】无人机状态量
    drone_state_pub = nh.advertise<uav_drone_msgs::DroneState>("/"+mav_name+"/drone_state", 10);

    //　【发布】无人机odometry，用于RVIZ显示
    odom_pub = nh.advertise<nav_msgs::Odometry>("/"+mav_name+"/drone_odom", 10);

    // 【发布】无人机移动轨迹，用于RVIZ显示
    trajectory_pub = nh.advertise<nav_msgs::Path>("/"+mav_name+"/drone_trajectory", 10);
    
    // 【发布】提示消息
    message_pub = nh.advertise<uav_drone_msgs::Message>("/"+mav_name+"/message/main", 10);

    // 【发布】UAV的角速度、姿态角（四元数形式）
    attitude_target_pub = nh.advertise<uav_drone_msgs::AttitudeTarget_replace>("/drone_pos_estimator/setpoint_raw/target_attitude", 10);
    // 【发布】UAV的速度、位置（XYZ坐标形式）
    position_target_pub = nh.advertise<uav_drone_msgs::PositionTarget_replace>("/drone_pos_estimator/setpoint_raw/target_local", 10);
    // 10秒定时打印，以确保程序在正确运行
     test_pub = nh.advertise<geometry_msgs::Vector3>("/test", 10);
    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);

    // 用于与rotors_simulator通讯的类， 接收来至飞控的消息【rotors_simulator->state_from_rotors类->本程序】
    state_from_rotors _state_from_rotors;

    // 频率
    ros::Rate rate(rate_hz);

    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while (ros::ok())
    {
        //回调一次 更新传感器状态
        ros::spinOnce();

        // 将采集的机载设备的定位信息及偏航角信息发送至lee_position_controller_node，根据参数input_source选择定位信息来源
        send_to_fcu();
        // 发布无人机状态至其他节点，如drone_pos_controller.cpp节点
        pub_to_nodes(_state_from_rotors._DroneState);
        //  整理接收到的状态信息，并按照预期进行发布
        rate.sleep();
    }

    return 0;
}

void send_to_fcu()
{
    geometry_msgs::PoseStamped vision;

    //无
    if (input_source == 0)
    {
        ROS_INFO("NOW DONT have input source 0");
    }
    else if (input_source == 2)
    {
        vision.pose.position.x = pos_drone_gazebo[0];
        vision.pose.position.y = pos_drone_gazebo[1];
        vision.pose.position.z = pos_drone_gazebo[2];

        vision.pose.orientation.x = q_gazebo.x();
        vision.pose.orientation.y = q_gazebo.y();
        vision.pose.orientation.z = q_gazebo.z();
        vision.pose.orientation.w = q_gazebo.w();
    }
    vision.header.stamp = ros::Time::now();
    vision_pub.publish(vision);
}

void pub_to_nodes(uav_drone_msgs::DroneState State_from_fcu)
{
    // 发布无人机状态，具体内容参见 prometheus_msgs::DroneState
    Drone_State = State_from_fcu;
    Drone_State.header.stamp = ros::Time::now();
    drone_state_pub.publish(Drone_State);

    // 发布无人机当前odometry,用于导航及rviz显示
    nav_msgs::Odometry Drone_odom;
    Drone_odom.header.stamp = ros::Time::now();
    Drone_odom.header.frame_id = "world";
    Drone_odom.child_frame_id = "base_link";

    Drone_odom.pose.pose.position.x = Drone_State.position[0];
    Drone_odom.pose.pose.position.y = Drone_State.position[1];
    Drone_odom.pose.pose.position.z = Drone_State.position[2];

    // 导航算法规定 高度不能小于0
    if (Drone_odom.pose.pose.position.z <= 0)
    {
        Drone_odom.pose.pose.position.z = 0.01;
    }

    Drone_odom.pose.pose.orientation = Drone_State.attitude_q;
    Drone_odom.twist.twist.linear.x = Drone_State.velocity[0];
    Drone_odom.twist.twist.linear.y = Drone_State.velocity[1];
    Drone_odom.twist.twist.linear.z = Drone_State.velocity[2];
    odom_pub.publish(Drone_odom);

    // 发布无人机运动轨迹，用于rviz显示
    geometry_msgs::PoseStamped drone_pos;
    drone_pos.header.stamp = ros::Time::now();
    drone_pos.header.frame_id = "world";
    drone_pos.pose.position.x = Drone_State.position[0];
    drone_pos.pose.position.y = Drone_State.position[1];
    drone_pos.pose.position.z = Drone_State.position[2];

    drone_pos.pose.orientation = Drone_State.attitude_q;

    //发布无人机的位姿 和 轨迹 用作rviz中显示
    posehistory_vector_.insert(posehistory_vector_.begin(), drone_pos);
    if (posehistory_vector_.size() > TRA_WINDOW)
    {
        posehistory_vector_.pop_back();
    }

    nav_msgs::Path drone_trajectory;
    drone_trajectory.header.stamp = ros::Time::now();
    drone_trajectory.header.frame_id = "world";
    drone_trajectory.poses = posehistory_vector_;
    trajectory_pub.publish(drone_trajectory);



    uav_drone_msgs::AttitudeTarget_replace drone_ground_Attitude;
    drone_ground_Attitude.header.stamp = ros::Time::now();
    drone_ground_Attitude.header.frame_id = "world";

    drone_ground_Attitude.orientation.x = Drone_State.attitude_q.x;
    drone_ground_Attitude.orientation.y = Drone_State.attitude_q.y;
    drone_ground_Attitude.orientation.z = Drone_State.attitude_q.z;
    drone_ground_Attitude.orientation.w =Drone_State.attitude_q.w;

    drone_ground_Attitude.body_rate.x = Drone_State.attitude_rate[0];
    drone_ground_Attitude.body_rate.y = Drone_State.attitude_rate[1];
    drone_ground_Attitude.body_rate.z = Drone_State.attitude_rate[2];

    drone_ground_Attitude.thrust = 0.5;
    attitude_target_pub.publish(drone_ground_Attitude);

    // 发布ground_station所需的话题，即UAV的速度、位置（XYZ坐标形式）
    uav_drone_msgs::PositionTarget_replace drone_ground_position;
    drone_ground_position.header.stamp = ros::Time::now();
    drone_ground_position.header.frame_id = "world";

    drone_ground_position.position.x = Drone_State.position[0];
    drone_ground_position.position.y = Drone_State.position[1];
    drone_ground_position.position.z = Drone_State.position[2];

    drone_ground_position.velocity.x = Drone_State.velocity[0];
    drone_ground_position.velocity.y = Drone_State.velocity[1];
    drone_ground_position.velocity.z = Drone_State.velocity[2];

    drone_ground_position.yaw = Drone_State.attitude[0];
    drone_ground_position.yaw_rate = Drone_State.attitude_rate[2];
    position_target_pub.publish(drone_ground_position);

    geometry_msgs::Vector3 test_att;
    test_att.x = Drone_State.attitude[0];
    test_att.y = Drone_State.attitude[1];
    test_att.z = Drone_State.attitude[2];

    test_pub.publish(test_att);
}
